// RIT Robotic Systems 2011-3 Project: Blue Steel
// Matt Hodge and Max Macri
//
#include <Ping.h> 
#include <Movement.h>

 // Constants
 const int stop_inches = 9; // inches before an object to stop
 const int min_forwardTime = 2000; // (ms) Min Time to wander forward
 const int max_forwardTime = 4001; // (ms) (+mintime-1)
 const int min_turnTime = 500; // (ms) Min Time to turn while wandering
 const int max_turnTime = 1501; // (ms) (+mintime-1)
 const int max_irThreshold = 600; // (0-1024) Threashold to detect IR
 const boolean debug = false; // True=5v board, False=3.3v board
 
 // Pins
 int leftIRPin = A0;
 int rightIRPin = A3;
 int horz = A2;
 int vert = A1;
 int RM_EN = 10;
 int LM_EN = 11;
 int LFWD = 6;
 int LREV = 7;
 int RFWD = 8;
 int RREV = 9;
 int leftPin = 2;
 int centerPin = 3;
 int rightPin = 4;
 
 // Primatives
 // Behavoir State Selection
 boolean pressed = false;
 int state = 0;
 // Line
 int leftValue[2] = {0,0};
 int rightValue[2] = {0,0};
 int centerValue[2] = {0,0};
 // Wander
 int leftIRVal = 0;
 int rightIRVal = 0;
 long randMove = 0;
 long randTurn = 0;
 int hort_move = 0;
 // Remote
 int readVal,hVal,vVal;
 
 // Objects
 Ping ping = Ping(5,0,29); //Pin,in_offset,cm_offset
 Movement move = Movement(LFWD,LREV,RFWD,RREV,LM_EN,RM_EN);

//
// Setup on Startup or Reset
//
void setup() {
  // Input
  pinMode(13,INPUT);
  pinMode(leftPin, INPUT);
  pinMode(centerPin, INPUT);
  pinMode(rightPin, INPUT);

  state = 0;
  // Seed PRNG
  randomSeed(analogRead(0));

}

//
// Main Loop - Check Button for Behavior State:
// Line Follower, IR Wanderer, Remote Driven
//
void loop() {
  // Check button for switched state
  int temp = digitalRead(13);
  if(temp==HIGH && pressed == false){
    pressed = true;
    state += 1;
    state = state%3;
  }else if(temp == LOW && pressed == true){
    pressed = false;
  }
  
  // Run behavior state algorithm
  switch(state){
    case 0:
      line_follow();
      break;
    case 2:
      wander_ir();
      break;
    case 1:
      remote_drive();
      break;
  }
}

//
// Line Following Algorithm
//
void line_follow(){
  // set current LCR values to previous LCR values (only if line was previously spotted)
  if((leftValue[0]==0) || (centerValue[0]==0) || (rightValue[0] ==0)) {
    leftValue[1] = leftValue[0];
    centerValue[1] = centerValue[0];
    rightValue[1] = rightValue[0];
  }
  
  // read new LCR values
  leftValue[0] = digitalRead(leftPin);
  centerValue[0] = digitalRead(centerPin);
  rightValue[0] = digitalRead(rightPin);

  if((leftValue[0]==0) && (centerValue[0]==1) && (rightValue[0]==1)) {
    // Left
    move.setMove(85,25);
    move.setMove(85,25);
  }
  if((leftValue[0]==0) && (centerValue[0]==0) && (rightValue[0]==1)) {
    // Left/Center
    move.setMove(85,25);
    move.setMove(85,25);
  }
  if((leftValue[0]==1) && (centerValue[0]==0) && (rightValue[0]==1)) {
    // Center
    move.setMove(50,25);
    move.setMove(50,25);
  }  
  if((leftValue[0]==1) && (centerValue[0]==0) && (rightValue[0]==0)) {
    // Right/Center
    move.setMove(15,25);
    move.setMove(15,25);
  }
  if((leftValue[0]==1) && (centerValue[0]==1) && (rightValue[0]==0)) {
    // Right
    move.setMove(15,25);
    move.setMove(15,25);    
  }
  if((leftValue[0]==1) && (centerValue[0]==1) && (rightValue[0]==1)) {
    // No line found, check previous values
    if((leftValue[1]==1) && (centerValue[1]==1) && (rightValue[1]==0)) {
      // Previous value was Right
      move.setMove(25,50);
      move.setMove(25,50);
      while(digitalRead(centerPin)){
        delay(1);
      }
      move.setMove(50,25);
      move.setMove(50,25);
    }
    if((leftValue[1]==0) && (centerValue[1]==1) && (rightValue[1]==1)) {
      // Previous value was Left
      move.setMove(75,50);
      move.setMove(75,50);
      while(digitalRead(centerPin)) {
        delay(1);
      }
      move.setMove(50,25);
      move.setMove(50,25);
    }
  }
  delay(50);

  //delay(500);
}

//
// Wandering and IR Following Algorithm
//
void wander_ir() {
 int moveTime = 0;
  
  // move forward for a random time (mintime -to- (maxtime+mintime-1))
  randMove = random(max_forwardTime);
  randMove += min_forwardTime;
  
  // move forward for the generated time, avoiding objects, then turn
  while(moveTime < randMove){
    // check for objects
    ping.fire();
    
    // check IR
    leftIRVal = analogRead(leftIRPin);
    rightIRVal = analogRead(rightIRPin);
    
    if(ping.inches() <= stop_inches) {
      // Object within range, turn to aviod
      move.setMove(25,50);
      moveTime = 0;
      
    }else if( leftIRVal <= max_irThreshold && rightIRVal <= max_irThreshold) { 
      // IR source has been spotted ahead, move towards it
      if(leftIRVal <= 350 && rightIRVal <=350) {
        // Move Straight Forward todo:mhodge mmacri
        move.setMove(50,25);
        
      }else{
        // Turn while moving forward
        hort_move = (rightIRVal-leftIRVal+max_irThreshold)/(max_irThreshold/50)+25;
        move.setMove(hort_move,25);
        delay(10);
        move.setMove(hort_move,25);
      }      
      moveTime = 0;
      
    }else if( leftIRVal <= max_irThreshold){ 
      // IR source has been spotted left, turn left towards it
      move.setMove(75,50);
      moveTime = 0;
      
    }else if( rightIRVal <= max_irThreshold){ 
      // IR source has been spotted right, turn right towards it
      move.setMove(25,50);
      moveTime = 0;
      
    } else {
      // Move forward for random time     
      move.setMove(50,25);
      moveTime += 50;
    }
    delay(50);
  }
  randTurn = random(max_turnTime);
  randTurn += min_turnTime;
    
  // (need to call setMove twice)
  move.setMove(25,50);
  delay(50);
  move.setMove(25,50);
  delay(randTurn); 
}

//
// Remote XBEE Drive Algorithm
//
void remote_drive() {
  // Read horizontal and vertical values
  readVal = analogRead(horz);
  if(debug){
    //5v board
    hVal = map(readVal,3,562,100,0);
  }else{
    //3.3v board
    hVal = map(readVal,3,850,100,0);
  }
  if(hVal > 100)
    hVal = 100;
  if(hVal < 0)
    hVal = 0;
  readVal = analogRead(vert);
  if(debug){
    //5v board
    vVal = map(readVal,2,560,0,100);
  }else{
    //3.3v board
    vVal = map(readVal,3,850,0,100);
  }
  if(vVal > 100)
    vVal = 100;
  if(vVal < 0)
    vVal = 0;
  
  // Check PING))), avoid objects
  ping.fire();
  if(ping.inches() > stop_inches){
    move.setMove(hVal,vVal);
  }else if(vVal >= 50-move.deadzone){
    move.setMove(hVal,vVal);
  }else{
    move.setMove(50,50);
  }
    delay(10); 
}


